
%% Initializing
clear all
close all
clc

% Initial value: tensor is a 3d matrix which contains a X, Y and Z
% coordinate for every point in the grid.
N=7;
tensor = zeros(N,N,3);
for i=1:N,
    % X and Y coordinates are evenly spaced
    tensor(i,:,1)=linspace(-N/2+0.01,N/2-0.01,N);
    tensor(:,i,2)=linspace(-N/2+0.01,N/2-0.01,N);
end

% The starting height (Z coordinate) for every point is 1
tensor(:,:,3)=4;


% x0
x0=tensor2array(tensor);
plotcloth(x0,0.8);

%% Solving 
close all
options=optimset('Algorithm','sqp','MaxFunEvals',75000);
[sol,fval] = fmincon(@cloth2,x0,[],[],[],[],[],[],@cloth_eqconstr2,options);
plotcloth(sol,0.8)
fval
%%
figure;
tensie = array2tensor(sol);
surf(tensie(:,:,1),tensie(:,:,2),tensie(:,:,3));
hold on
% plot3([-N/2  N/2],[-N/2 N/2 ],[1 1],'^','MarkerSize',10, 'MarkerFaceColor', 'b');